/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef _CLineMapRawScanMCL__H_
#define _CLineMapRawScanMCL__H_

#include "CParticleFilter.h"
/*
#define VMAP 0.2					// Map uncertainty
#define VANG 0.2					// Angle uncertainty
#define VR	 0.02					// Uncertainty of the range measurement
#define FALSE_PROB 0.3				// "Probability" of the false measurement

*/
#define VMAP 0.1					// Map uncertainty
#define VANG 0.1					// Angle uncertainty
#define VR	 0.02					// Uncertainty of the range measurement
#define FALSE_PROB 0.2				// "Probability" of the false measurement


#define VMEAS ((VMAP+VANG+VR)*(VMAP+VANG+VR)) // Whole measurement uncertainty

//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
/// The Line Map used by the MCL
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
/**
 * Structure for presenting line in form of a*x + b*y + c = 0
 */
struct TLineParam{
		public:	
				float a; 
				float b;
				float c;
				TLineParam(){
						a=0;b=0;c=0;
				}
};

/**
 * Structure to present single line with end points
 */
struct TLine{
		public:
				float x1,y1,x2,y2;
				TLineParam p;
				bool is_param_ok;
				TLine(){
						x1=0;x2=0;y1=0;y2=0;
						is_param_ok=false;
				}
		
		/**
				 * Convert line(end point, start point) to a * x + b * y + c = 0 form
		 **/ 
				void setLineParam(){
						p.a = y2 - y1;
						p.b = x1 - x2;
						p.c = (-p.a) * (x1) - (p.b) * (y1);
						is_param_ok = true;
				}
		
		/**
				 * Calculate lenght of the line
				 *@param l line
				 *@return float The lenght of the line
		 */
				float lenght(){
						return sqrt((x1-x2) * (x1-x2) + (y1-y2) * (y1-y2));
				}
		/**
				 * Returns the angle of line 0..PI
				 *@param l 
				 *@return angle of line
		 */
				float angle(){
						float dx, dy;
						float angle;
						dx = x1 - x2;
						dy = y1 - y2;
						angle = (float)atan2(dy, dx);
						if(angle<0) angle+=(float)M_PI;
						return angle;
				}
		/**
				 * Calculates the distance of the point (x,y) from line L
				 * @return distance if succesful, else -1
		 */
				float pointDistanceFromLine(float x, float y){
						if(!is_param_ok){
								this->setLineParam();
						}
						if(sqrt(p.a*p.a + p.b*p.b)>0)
								return fabs( p.a * x + p.b * y + p.c )/sqrt(p.a*p.a + p.b*p.b);
						else return -1;
				}	
		/** 
				 * Returns the  angle difference between two lines
				 *
		 **/
				float angleDiff(TLine l){
						float dx1, dy1;
						float dx2, dy2;
						dx1 = x1 - x2;
						dy1 = y1 - y2;
						dx2 = l.x1 - l.x2;
						dy2 = l.y1 - l.y2;
						return acos(fabs (dx1*dx2+dy1*dy2) / ( sqrt(dx1*dx1+dy1*dy1) * sqrt(dx2*dx2+dy2*dy2) ) );
				}
		
		/**
				 * Rotates line around point
				 * @p x the x-coordinate 
				 * @p y the y-coordinate
				 * @param alpha rotation angle
		 */
				void rotateLine(float x, float y, float alpha){
						float cos_alpha = cos(alpha);
						float sin_alpha = sin(alpha);
						float x11,x22,y11,y22;
			
						x11 = cos_alpha * (x1 - x) - sin_alpha * (y1 - y) + x;
						y11 = sin_alpha * (x1 - x) + cos_alpha * (y1 - y) + y; 
 
						x22 = cos_alpha * (x2 - x) - sin_alpha * (y2 - y) + x;
						y22 = sin_alpha * (x2 - x) + cos_alpha * (y2 - y) + y;
						x1=x11;x2=x22;
						y1=y11;y2=y22;
				}
			
			/**
				 * int isInLine(double x0,double y0,TLine Line)
				 * checks if coordinate x0, y0 belongs to the line Line
				 * Returns 1 if it does 0 if not
			 */
				int isInLine(float x0,float y0){
						float mx1,mx2,my1,my2;
						float L,AC,AB;
						mx2=x1;my2=y1; mx1=x2; my1=y2; 
				
						L=( (mx2-mx1)*(mx2-mx1) + (my2-my1)*(my2-my1) );
						AC=( (x0-mx2)*(x0-mx2) + (y0-my2)*(y0-my2) );
						AB=( (x0-mx1)*(x0-mx1) + (y0-my1)*(y0-my1) );
	
						if(AC<=L && AB<=L ) return 1; // yes it is in line
						else 
								return 0;
				}
				/**
				 * Calculates point (coordinate) from the line, which is closest 
				 * to the position x,y
				 **/  				
				void shortestPointFromLine(float x,float y,float &ret_x, float &ret_y){
						float k,b;
					
						ret_x = 0;
						ret_y = 0;
					
						if(fabs(p.b)> 0.00001 && fabs(p.a)>0.00001){
								k = p.b/p.a; //Paitsi jos b=0
								b = y - k * x;
		
								ret_x = -(b*p.b+p.c)/(p.a+p.b*k);
								ret_y = k*ret_x+b;
						}
						else if( fabs(p.b)  < 0.00001){
								ret_x = -p.c/p.a;
								ret_y = y;
						}
						else{
								ret_x = x;
								ret_y = -p.c/p.b;
						}
				}
				
				/**
				 * Checks if the point (x0,y0) is close to the 
				 * line end points. Used for Local map determination.
				 * 
				 * Returns 1 if closer than r else 0
				 **/

				int isCloseToEndPoints(float x0,float y0, float r){
						double mx1,mx2,my1,my2;
						double AC,AB;
	
						mx2=x1;my2=y1; mx1=x2; my1=y2; 

						AC = (mx1-x0)*(mx1-x0) + (my1-y0)*(my1-y0);
						AB = (mx2-x0)*(mx2-x0) + (my2-y0)*(my2-y0);

						if (AC < r*r || AB<r*r)
								return 1;
						else return 0;
				}
};

struct TLineMap{
		public:
TLine *Lines;
int NumOfLines;
int size;

TLineMap(){
		Lines = NULL;
		NumOfLines = 0;
		size=0;
}

void allocate(int m_size){
		if(Lines) free(Lines);
		Lines = (TLine *) malloc(m_size * sizeof(TLine));
		NumOfLines=0;
		size = m_size;
}

/**
		* reads the linemap from file
		* Map is in format Nx4 (%f %f %f\n...)
		* scale is the conversion scale between the units in file and meter
		* e.g. usually cad drawings are in mm:s so the conversion factor is 1/1000
		* 
		* returns 0 on success, -1 on failure
		*/
int readMapFile(const char *fileName,float scale){
		FILE *mapfile	  = fopen(fileName,    "r");	   // Map data as line
		int i=0;
		TLine tmp;
		float map[4];
		std::vector<TLine> mapVector;
		
		if(mapfile==NULL){
				fprintf(stderr,"readMapFile()::Map file not found\n");
				return -1;
		}
		while(!feof(mapfile)) {
				for(i = 0; 4 > i; i++){
						fscanf(mapfile, "%f ", &map[i]); ///<Read one line of line data
						map[i]=map[i]*scale; ///< Scale
				}
				tmp.x1 = map[0];tmp.y1 = map[1];tmp.x2 = map[2];tmp.y2 = map[3];
				tmp.setLineParam();
				mapVector.push_back(tmp);
		}
		fclose(mapfile);

		this->allocate(mapVector.size());

		for(i = 0; i<this->size ; i++) {
				Lines[i]=mapVector[i];
		
		}
		
		NumOfLines = size;
		fprintf(stderr,"readMapFile()::Called and read %d (mv=%d) lines\n",NumOfLines,mapVector.size());
		return 0;
}
		
		/**
		* Can set the float *data to the linemap format
		* @p *data pointer to data in format [x1y1x2y2...]
		*  @p size the size of the data in total (size = 4*sizeof(float)*NumberOfLines)
**/
int setDataLumb(float *data, int size){
		int N,i;

		N = size / (4*sizeof(float)); ///< number of lines in the *data
		this->allocate(N);
		for(i=0;i<N;i++){
				Lines[i].x1 = data[4*i];
				Lines[i].y1 = data[4*i+1];
				Lines[i].x2 = data[4*i+2];
				Lines[i].y2 = data[4*i+3];
				Lines[i].setLineParam();
		}
		return 0;
}
		/**
		* Transforms the data into dataLumb described above
		* @p *data pointer to data (allocated in the function)
		* @p &d_size the data size is stored here d_size = NumOfLines * sizeof(float)*4
*/
void getDataPtr(float *data, int &d_size){
		int i;
		data = (float *) malloc(NumOfLines * sizeof(float)*4);
		d_size = NumOfLines * sizeof(float)*4;
		for(i=0;i<NumOfLines;i++){
				data[4*i]=Lines[i].x1;
				data[4*i+1]=Lines[i].y1;
				data[4*i+2]=Lines[i].x2;
				data[4*i+3]=Lines[i].y2;
		}
}

void saveXML(const char *filename,const char *main_tag, const char *linetag){
		FILE *xmlfile	  = fopen(filename,"wt");
		if(!xmlfile){
				fprintf(stderr,"saveXML::Error opening file %s\n",filename);
				return;
		}
		fprintf(xmlfile,"<%s>\n",main_tag);
		for(int i=0;i<NumOfLines;i++){
				fprintf(xmlfile,"<%s x1=\"%.3f\" y1=\"%.3f\" x2=\"%.3f\" y2=\"%.3f\" />\n",linetag,
								Lines[i].x1,
								Lines[i].y1,
								Lines[i].x2,
								Lines[i].y2);
		}
		fprintf(xmlfile,"</%s>\n",main_tag);
		fclose(xmlfile);
}

     
/**
		* Finds local map around (x0,y0) from map and stores the 
		* result into lmap. lmap can be initialized, if not, the 
		* lmap will be set to same size as actual map
		* The lmap radius is used to determine the lines that belongs to local map
*/

int getLocalMap(float x0,float y0, float lmap_radius, TLineMap &lmap){
		int i;
		int isIt;
		float rx,ry;

		if(lmap.size <= 0){ ///<The local map is uninitialized
				lmap.allocate(NumOfLines); 
		}
		lmap.NumOfLines = 0; ///< reset the vector

		for(i=0;i<NumOfLines;i++){
				if(lmap.NumOfLines >=lmap.size){
						fprintf(stderr,"getLocalMap():: Error: The size of the local map is too small\n");
						return -1;
				}
				if(Lines[i].pointDistanceFromLine(x0,y0) < lmap_radius){
						Lines[i].shortestPointFromLine(x0,y0, rx, ry);
						isIt = Lines[i].isInLine(x0,y0);

						if(isIt==1){
								lmap.Lines[lmap.NumOfLines]=Lines[i];
								lmap.NumOfLines++;
						}
						else{
								if(Lines[i].isCloseToEndPoints(x0,y0,lmap_radius)==1){
										lmap.Lines[lmap.NumOfLines]=Lines[i];
										lmap.NumOfLines++;
								}
						}//ELSE
				}//IF POINTDISTANCE

		}// FOR

		return 0; 
}
		
		/** 
				* Calculates virtual scan from line map
				* 
				* 
				* @param *r	Output: The range measurements. If not used set as NULL. 
				*						The size should be same as @number_of_measurements
				* @param *xyscan_rel	Output: The relative x,y coordinates.If not used set as NULL.
		The size should be same as 2 * @number_of_measurements
				* @param *xyscan_abs	Output: The absolute x,y coordinates.If not used set as NULL.
				*						          The size should be same as 2 * @number_of_measurements
				* @param *scanLines		The calculated scanlines, size must be 3 * number_of_measurements
				* 
				* @param Pos			The position where the robot is assumed to be
				* @param map			The linemap
				*
				* @param number_of_measurements The number of laser beams to be calculated
				* @param laserOffset	The angle difference between the robot coordinate system and laser coordinate system.
				*						e.g. If your robot is travelling along X-axis in zero heading, the laser is probably 
				*						pointing forward, thus the first measurement (0 angle from laser point of view) is from 
		-PI/2 from the robot perspective. In this case the offset is -PI/2
				*
		*/
int virtualScanner(float *r, ///<out: distance "measurements"
										float *xyscan_rel, ///< out: measurements in relative x,y-frame
										float *xyscan_abs, ///< out: measurements in absolute x,y-frame
										float *scanLines,  ///< out: The line parameters of the scan
										float px0,float py0,float pa0,    ///< Position where the scan is taken from
										int number_of_measurements, ///<Number of measurements taken
										float laserOffset){         ///<offset between laser and robot


		int i,j,start,end;
		int outliers=0;
		float a1,b1,c1,a2,b2,c2; //Line parameters
		float x0,y0,px=100,py=100;
		float mx1,my1,mx2,my2;
		float rTmp[512]; //temporarily store the results here (note max number of meas is 512)
		float phii,alpha;
		float range,fac,ifac,num;

		if(number_of_measurements > 511 || number_of_measurements<0){
				fprintf(stderr,"findVirtualScan2:: Invalid number of measurements\n");
				r = NULL;
				xyscan_rel = NULL;
				xyscan_abs = NULL;
				return -1;
		}
		if(scanLines==NULL){
			fprintf(stderr,"virtualScanner()::Allocating memory for the scanlines\n!!!");
				scanLines = (float *) malloc(3*sizeof(float)*number_of_measurements);
		}

		fac=number_of_measurements/M_PI;
		ifac=1/fac;
		// Rotate to the laser coordinate system
		phii=pa0 + laserOffset;

		while(phii < -M_PI) {
				phii += (float)(2.0f*M_PI);
		}
		while(phii > M_PI) {
				phii -= (float)(2.0f*M_PI);
		}

		for(i=0;i<number_of_measurements;i++)
		{
				rTmp[i]= 81.5f*81.5f;
				alpha=(float)phii+(float)(((float)i)*ifac) ;
				scanLines[3*i]=(float)-sin(alpha);
				scanLines[3*i+1]=(float)cos(alpha);
				scanLines[3*i+2]=(float)(-scanLines[3*i]*px0-scanLines[3*i+1]*py0);
		}

		for(i=0;i<NumOfLines;i++){

				mx2=Lines[i].x1; //map[i*N];
				my2=Lines[i].y1; //map[i*N+1];
				mx1=Lines[i].x2; //map[i*N+2];
				my1=Lines[i].y2; //map[i*N+3];

				a2=Lines[i].p.a; // Lines[i*3];
				b2=Lines[i].p.b; // Lines[i*3+1];
				c2=Lines[i].p.c; // Lines[i*3+2];

				px=(mx1-px0);
				py=(my1-py0);

				//calculate angles at which the endpoints are seen
				start=(int)floor((atan2(py,px)-phii)*fac);

				px=(mx2-px0);
				py=(my2-py0);
				end=(int)floor((atan2(py,px)-phii)*fac);

				if(end<0)
						end=(number_of_measurements * 2 - 1)+end;
				if(start<0)
						start=(number_of_measurements * 2 - 1)+start;
				if(end<start)
				{
						j=start;
						start=end;
						end=j;
				}
				if((end-start)>number_of_measurements-1)
				{
						end=start;
						start=0;
				}
				if(end>number_of_measurements)
						end=number_of_measurements; 
				 
				if(end < 0 || start<0){
					//fprintf(stderr,"Virtualscanner():: Very strange problem since start=%d and end =%d\n",start,end);
					end = number_of_measurements;
					start = 0;
				} 
				for(j=start;j<end;j++){
						 //fprintf(stderr,"J=%d ",j);
						a1=scanLines[3*j];
						b1=scanLines[3*j+1];
						c1=scanLines[3*j+2];
						num=1/(a1*b2-a2*b1);
								//Calculation of intersection
						x0 = (c2*b1-c1*b2)*num ;
						y0 = (a2*c1-c2*a1)*num;

						px=(x0-px0); // Relative coordinates
						py=(y0-py0); // Relative coordinates
						range=(px*px + py*py);
						if(rTmp[j]>range)
						{
								rTmp[j]=range;
						// Store the relative x,y-coordinates also
								if(xyscan_rel != NULL){
										xyscan_rel[2*j]=	px;
										xyscan_rel[2*j+1] = py;
								}
								// Store the absolute x,y-coordinates also
								if(xyscan_abs != NULL){
										xyscan_abs[2*j]=	x0;
										xyscan_abs[2*j+1] = y0;
								}

						}
				}
		}

		if(r != NULL){
				for(i=0;i<number_of_measurements;i++){
						r[i]=(float)sqrt(rTmp[i]);
						if(r[i]>80.0f) outliers++;
				}
		}

		return outliers;
}

/**
* Returns the corners of the bounding box (diagonal line actually)
* of the map
*/
TLine getMapBoundingBox(){
	int i;
	float maxX=-1000000,minX=100000;
	float maxY=-1000000,minY=100000;
	TLine ret;
	
	for(i=0;i<NumOfLines;i++){
		if(Lines[i].x1>maxX) maxX = Lines[i].x1;
		if(Lines[i].x2>maxX) maxX = Lines[i].x2;
		
		if(Lines[i].x1 < minX) minX = Lines[i].x1;
		if(Lines[i].x2 < minX) minX = Lines[i].x2;
		
		if(Lines[i].y1>maxY) maxY = Lines[i].y1;
		if(Lines[i].y2>maxY) maxY = Lines[i].y2;
		
		if(Lines[i].y1 < minY) minY = Lines[i].y1;
		if(Lines[i].y2 < minY) minY = Lines[i].y2;
	}
	ret.x1 = minX;
	ret.y1 = minY;
	ret.x2 = maxX;
	ret.y2 = maxY;
	return ret;
}

	
};



class CLineMapRawScanMCL{
		public:
				CParticleFilter filt; ///<the particle filter used
				TLineMap map;
				
				CLineMapRawScanMCL();
				~CLineMapRawScanMCL();
				/**
				 * Initializes the filter - uses normal random method for calculating the initial distribution
				 * Loads the map from file, reserves memory for particles, sets the initial distribution
				 * @p *mapfile The mapfile name
				 * @p mapscale the scale factor needed to get the map coordinates into meters
				 * @p num_of_particles The number of particles in the filter
				 * @p init_pose the initial pose
				 * @p initVariance the variance for the initial quess
				 * @p num_of_meas_in_scan number of measurements used from the scan
				 */
				void initialize(const char *mapfile,
														float mapscale, 
														int num_of_particles, 
														mcl::pose initPose, 
														mcl::pose initVariance,
														int num_of_meas_in_scan,
														float _sensor_offset);
				
				/**
				 * Initializes the filter - uses uniform distribution method for calculating the initial distribution
				 * Loads the map from file, reserves memory for particles, sets the initial distribution
				 * The initial distribution is calculated using bounding box of the map. The @dP sets the 
				 * resolution of the unform grid. WARNING the initialization might not be 
				 * real-time.
				 * @p *mapfile The mapfile name
				 * @p mapscale the scale factor needed to get the map coordinates into meters
				 * @p dP the resolution of the uniform grid
				 * @p num_of_meas_in_scan number of measurements used from the scan
				 */
				void initializeGlobal(const char *mapfile,
												float mapscale, 
												mcl::pose dP, 
												int num_of_meas_in_scan,
												float _sensor_offset);

				/**
				* Same as init but only for the filter
				* Initializes the filter - uses normal random method for calculating the initial distribution
				*/
				void reinitializeNR(mcl::pose initPose,mcl::pose initVariance);
				
				/**
				* Run the MCL step
				* @p dP the estimated relative movement between steps (prediction)
				* @p variance the error of prediction
				* @p measurement the scan taken from new position
				*/
				void runMCL(mcl::pose dP, mcl::pose variance, mcl::scan measurement);
				
				/**
				* Run the MCL step in initialization mode
				* @p dP the estimated relative movement between steps (prediction)
				* @p variance the error of prediction
				* @p measurement the scan taken from new position
				*/
				void runMCLInit(mcl::pose dP, mcl::pose variance, mcl::scan measurement);
				
				void getScan(mcl::scan &meas, mcl::pose &p);
		private:
				
				float *virtualr;
				float *virtuallines;
				float *virtualscan_xy;
				float *virtualscan_xy_rel;
				int num_of_meas;
				float sensor_offset;
				
				
				/**
				* Calculates the likelihood of particle distribution
				* uses the CParticleFilter filt and TLineMap map
				* @param meas The current measurement
				**/
				void calculateLikelihood(mcl::scan meas);
				
				/**
				* Initialises the variables
				*/
				void initializeVariables(const char *mapfile,
																 float mapscale, 
																 int num_of_meas_in_scan,
																 float _sensor_offset);
				
};

#endif


